Graph Optimization with Unstructured Covariance: Fast, Accurate, Linear Approximation

This manuscript addresses the problem of optimizationbased Simultaneous Localization and Mapping (SLAM), which is of concern when a robot, traveling in an unknown environment, has to build a world model, exploiting sensor measurements. Although the optimization problem underlying SLAM is nonlinear and nonconvex, related work showed that it is possible to compute an accurate linear approximation of the optimal solution for the case in which measurement covariance matrices have a block diagonal structure. In this paper we relax this hypothesis on the structure of measurement covariance and we propose a linear approximation that can deal with the general unstructured case. After presenting our theoretical derivation, we report an experimental evaluation of the proposed technique. The outcome confirms that the technique has remarkable advantages over state-of-the-art approaches and it is a promising solution for large-scale mapping.